#include <OpenCV/cv.h>
#include <OpenCV/highgui.h>

#include "osc/OscOutboundPacketStream.h"
#include "ip/UdpSocket.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>

#ifdef _EiC
#define WIN32
#endif

// OSC
#define ADDRESS "127.0.0.1"
#define PORT 7000
#define OUTPUT_BUFFER_SIZE 1024


#define CAPTURE_AFTER 8 





UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];

static CvMemStorage* storage = 0;
static CvHaarClassifierCascade* cascade = 0;

int contCount = 0;

#define CONT 25;

void detect_and_draw( IplImage* image );
 char* capture_file = "~/tmp/cv_capture.jpg";

const char* cascade_name =
    "haarcascade_frontalface_alt.xml";
/*    "haarcascade_profileface.xml";*/


int main( int argc, char** argv )
{
	
	capture_file = "/tmp/cv_capture.jpg";

    CvCapture* capture = 0;
    IplImage *frame, *frame_copy = 0;
    int optlen = strlen("--cascade=");
    const char* input_name;

    if( argc > 1 && strncmp( argv[1], "--cascade=", optlen ) == 0 )
    {
        cascade_name = argv[1] + optlen;
        input_name = argc > 2 ? argv[2] : 0;
    }
    else
    {
        cascade_name = "haarcascade_frontalface_alt2.xml";
        input_name = argc > 1 ? argv[1] : 0;
    }

    cascade = (CvHaarClassifierCascade*)cvLoad( cascade_name, 0, 0, 0 );
    
    if( !cascade )
    {
        fprintf( stderr, "ERROR: Could not load classifier cascade\n" );
        fprintf( stderr,
        "Usage: facedetect --cascade=\"<cascade_path>\" [filename|camera_index]\n" );
        return -1;
    }
    storage = cvCreateMemStorage(0);
	
    if( !input_name || (isdigit(input_name[0]) && input_name[1] == '\0') )
        capture = cvCaptureFromCAM( !input_name ? 0 : input_name[0] - '0' );
    else
        capture = cvCaptureFromAVI( input_name );

	/*
	cvSetCaptureProperty (capture, CV_CAP_PROP_FPS, 30);
	cvSetCaptureProperty (capture, CV_CAP_PROP_FRAME_WIDTH, 320);
	cvSetCaptureProperty (capture, CV_CAP_PROP_FRAME_HEIGHT, 240);
	*/
	
    cvNamedWindow("ICSY TK 5000 - mission control 0.1 alpha", 0 );
	//cvWaitKey(500);
	//cvResizeWindow( "result", 320, 240 );
	

    if( capture )
    {
        for(;;)
        {
            if( !cvGrabFrame( capture ))
                break;
            frame = cvRetrieveFrame( capture );
            if( !frame )
                break;
            if( !frame_copy )
                frame_copy = cvCreateImage( cvSize(frame->width,frame->height),
                                            IPL_DEPTH_8U, frame->nChannels );
            if( frame->origin == IPL_ORIGIN_TL )
                cvCopy( frame, frame_copy, 0 );
            else
                cvFlip( frame, frame_copy, 0 );
            
            detect_and_draw( frame_copy );

            if( cvWaitKey( 10 ) >= 0 )
                break;
        }

        cvReleaseImage( &frame_copy );
        cvReleaseCapture( &capture );
    }
    else
    {
        const char* filename = input_name ? input_name : (char*)"lena.jpg";
        IplImage* image = cvLoadImage( filename, 1 );

        if( image )
        {
            detect_and_draw( image );
            cvWaitKey(0);
            cvReleaseImage( &image );
        }
        else
        {
            /* assume it is a text file containing the
               list of the image filenames to be processed - one per line */
            FILE* f = fopen( filename, "rt" );
            if( f )
            {
                char buf[1000+1];
                while( fgets( buf, 1000, f ) )
                {
                    int len = (int)strlen(buf);
                    while( len > 0 && isspace(buf[len-1]) )
                        len--;
                    buf[len] = '\0';
                    image = cvLoadImage( buf, 1 );
                    if( image )
                    {
                        detect_and_draw( image );
                        cvWaitKey(0);
                        cvReleaseImage( &image );
                    }
                }
                fclose(f);
            }
        }

    }
    
    cvDestroyWindow("result");

    return 0;
}

void detect_and_draw( IplImage* img )
{
    static CvScalar colors[] = 
    {
        {{0,0,255}},
        {{0,128,255}},
        {{0,255,255}},
        {{0,255,0}},
        {{255,128,0}},
        {{255,255,0}},
        {{255,0,0}},
        {{255,0,255}}
    };

    double scale = 3; //1.3;
    IplImage* gray = cvCreateImage( cvSize(img->width,img->height), 8, 1 );
    IplImage* small_img = cvCreateImage( cvSize( cvRound (img->width/scale),
                         cvRound (img->height/scale)),
                     8, 1 );

//		        cvSetImageROI(small_img, cvRect(1000,1000, small_img->width/2,small_img->height/2));
	
    int i;

    cvCvtColor( img, gray, CV_BGR2GRAY );
    cvResize( gray, small_img, CV_INTER_LINEAR );
    cvEqualizeHist( small_img, small_img );
    cvClearMemStorage( storage );

    if( cascade )
    {
        double t = (double)cvGetTickCount();
        CvSeq* faces = cvHaarDetectObjects( small_img, cascade, storage,
                                            1.1, 2, 0/*CV_HAAR_DO_CANNY_PRUNING*/,
                                            cvSize(20, 20) );
        t = (double)cvGetTickCount() - t;
        //printf( "detection time = %gms\n", t/((double)cvGetTickFrequency()*1000.) );
		
		int maxradius = 0;
		int x = 0;
		int y = 0;
		
        for( i = 0; i < (faces ? faces->total : 0); i++ )
        {
			CvRect* r = (CvRect*)cvGetSeqElem( faces, i );
            CvPoint center;
            int radius;
            center.x = cvRound((r->x + r->width*0.5)*scale);
            center.y = cvRound((r->y + r->height*0.5)*scale);
            radius = cvRound((r->width + r->height)*0.25*scale);
			
			if (radius > maxradius)
			{
				x = center.x;
				y = center.y;
				maxradius = radius;
			}
            cvCircle( img, center, radius, colors[i%8], 3, 8, 0 );
        }
		
		if (maxradius != 0)
		{
			contCount++;	
			
		osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
		p << osc::BeginBundleImmediate
				<< osc::BeginMessage( "/x" )
					<< x << osc::EndMessage
				<< osc::BeginMessage( "/y" )
					<< y << osc::EndMessage
				<< osc::BeginMessage( "/radius" )
					<< maxradius << osc::EndMessage
				<< osc::EndBundle;
		transmitSocket.Send( p.Data(), p.Size() );
		printf("x: %d, y: %d, r: %d, c:%d\n\r", x, y, maxradius, contCount);
	
		
		} else {
			
			contCount = 0;
		}
		
		
		if(contCount == CAPTURE_AFTER){
			
			int Day;
			  time_t TotSec;
			  struct tm * TimeStruct;
			
			     TotSec = time( NULL );
			    char       buf[80];
			
				printf("%d", TotSec);
				
				sprintf(buf, "/tmp/icsy_capture_%d.jpg", TotSec);
				cvSaveImage(buf, gray );
			
				osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
				p << osc::BeginBundleImmediate
						<< osc::BeginMessage("/capture")
							<< x << y << maxradius << buf << osc::EndMessage
						<< osc::EndBundle;
				transmitSocket.Send( p.Data(), p.Size() );
				printf("### Capture made to %s.\n", buf);
				printf("x: %d, y: %d, r: %d, c:%d\n\r", x, y, maxradius, contCount);	
		}
    }

    cvShowImage( "ICSY TK 5000 - mission control 0.1 alpha", img );
	//cvReleaseImage( &img );
    cvReleaseImage( &gray );

 	cvResetImageROI(small_img);
    cvReleaseImage( &small_img );
}
